#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/Empty.h>
#include <std_msgs/String.h> 
#include <std_msgs/Header.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include "radio_serial/Cmd_send.h"
#include "radio_serial/Uav_state.h"
#include <thread>
#define STATE_UAV 101
#define CMD_UAV 102
#define SELF_CMD 103
#define ARM 1
#define TAKEOFF 2
#define LAND 3
#define POS 4
#define VEL 5
#define ACC 6
#define POS_VEL 7
#define POS_ACC 8
#define VEL_ACC 9
#define POS_VEL_ACC 10
#define OPEN_TRA 11

#define LENTH_STATE_UAV 51
#define LENTH_CMD_UAV 49

static uint8_t s_buffer[LENTH_STATE_UAV];	//分配静态存储空间
static uint8_t recv_buffer[LENTH_CMD_UAV];	//分配静态存储空间
serial::Serial ser; //声明串口对象 

float Buf2Float(unsigned char *buff)
{
    float xval;
    unsigned int i;
    unsigned char *S;
    S = (unsigned char *)(&xval);
    for (i=0;i<4;i++) *(S+i) = *(buff+i);
    return xval;
}

int check_sum(unsigned char *buff, int len)
{
    int sum = 0;
    for (int i=0; i<len; i++)
    {
        sum = (sum + buff[i]) & 0xffffffff;
    }
    return sum & 0xffffffff;
}

//回调函数 
void write_callback(const radio_serial::Uav_state::ConstPtr& msg) 
{ 
      // ROS_INFO_STREAM("Writing to serial port \n" << *msg); 
      memset(s_buffer,0,sizeof(s_buffer));
      s_buffer[0] = 0xff;
      s_buffer[1] = 0xfe;
      s_buffer[2] = (msg->frame_id & 0xff);     //frame_id
      s_buffer[3] = msg->uav_index;      //uav_index
      s_buffer[4] = msg->connected;      //connected
      s_buffer[5] = msg->armed;    //armed
      s_buffer[6] = (msg->mode & 0xff);      //mode
      // memcpy((s_buffer+7), &(msg->latitude), 8);     //latitude
      // memcpy((s_buffer+15), &(msg->longitude), 8);    //longitude
      // memcpy((s_buffer+23), &(msg->altitude), 8);     //altitude
      // memcpy((s_buffer+31), &(msg->rel_altitude), 8);  //rel_altitude
      // memcpy((s_buffer+39), &(msg->deg_compass), 8); //deg_compass
      // memcpy((s_buffer+47), &(msg->local_x), 8);     //local_x
      // memcpy((s_buffer+55), &(msg->local_y), 8);     //local_y
      // memcpy((s_buffer+63), &(msg->local_z), 8);     //local_z
      // memcpy((s_buffer+71), &(msg->local_lin_x), 8); //local_lin_x
      // memcpy((s_buffer+79), &(msg->local_lin_y), 8); //local_lin_y
      // memcpy((s_buffer+87), &(msg->local_lin_z), 8); //local_lin_z
      // memcpy((s_buffer+95), &(msg->ground_speed), 8); //ground_speed
      // memcpy((s_buffer+103), &(msg->air_speed), 8); //air_speed

      memcpy((s_buffer+7), &(msg->local_x), 8);     //local_x
      memcpy((s_buffer+15), &(msg->local_y), 8);     //local_y
      memcpy((s_buffer+23), &(msg->local_lin_x), 8); //local_lin_x
      memcpy((s_buffer+31), &(msg->local_lin_y), 8); //local_lin_y
      memcpy((s_buffer+39), &(msg->deg_compass), 8); //deg_compass
      // float time = 0;
      // memcpy((s_buffer+47), &(time), 8); //ros_time

      // time_t tt = time(NULL);
      // tm* t = localtime(&tt);
      // int hour = t->tm_hour;
      // int minutes = t->tm_min;
      // int seconds = t->tm_sec;
      // memcpy((s_buffer+111), &hour, 4);
      // memcpy((s_buffer+115), &minutes, 4);
      // memcpy((s_buffer+119), &seconds, 4);
      // std::cout << "hour=" << hour << std::endl;
      // std::cout << "minutes=" << minutes << std::endl;
      // std::cout << "seconds=" << seconds << std::endl;
      
      int sum_end = check_sum(s_buffer, LENTH_STATE_UAV-4);
      // std::cout << "sum_end=" << sum_end << std::endl;

      memcpy((s_buffer+LENTH_STATE_UAV-4), &sum_end, 4);
      ser.write(s_buffer, LENTH_STATE_UAV);   //发送串口数据
}

void Decode_frame(serial::Serial& ser, ros::Publisher& read_pub, ros::Publisher& time_delay_pub)
{
      radio_serial::Cmd_send result;
      std_msgs::Int32 time_delay;
      static std::vector<uint8_t> last_remaining_buffer;
      size_t bytes_read;

      // std::cout << "ser.available()=" << ser.available() << std::endl;
      // std::cout << "last_remaining_buffer.size()=" << last_remaining_buffer.size() << std::endl;


      while(true){
            std::vector<uint8_t> cat_buffer;

            // std::cout << "bytes_read=" << bytes_read << std::endl;
            // std::cout << "recv_buffer.size()=" << sizeof(recv_buffer) << std::endl;
            // std::cout << "ser.available()=" << ser.available() << std::endl;
            bytes_read = ser.read(recv_buffer, LENTH_CMD_UAV);

            // cat_buffer.reserve(last_remaining_buffer.size() + bytes_read); //预留空间

            cat_buffer.insert(cat_buffer.end(), last_remaining_buffer.begin(), last_remaining_buffer.end()); //将last_remaining_buffer中的数据插入到cat_buffer中
            cat_buffer.insert(cat_buffer.end(), recv_buffer, recv_buffer+bytes_read); //继续将recv_buffer中的数据插入到cat_buffer中

            // std::cout << "cat_buffer.size()=" << cat_buffer.size() << std::endl;

            int data_len = cat_buffer.size();

            // std::cout << "data_len=" << data_len << std::endl;

            // 找包头
            int i = 0;
            for(i; i < data_len-2; i++){
                  if(cat_buffer[i] ==0xff && cat_buffer[i+1] == 0xfe){
                        if(data_len - i + 1 >= LENTH_CMD_UAV){// 判断是否存在完整帧
                  
                              int sum_check, sum_end;
                              // 求和校验
                              sum_check = check_sum(cat_buffer.data(), LENTH_CMD_UAV-4);
                              // 根据包头确定校验位和包尾是否符合协议满足协议要求
                              memcpy(&sum_end, &cat_buffer[LENTH_CMD_UAV-4], 4); //sum_end

                              if(sum_check == sum_end){
                              // std::cout << "1111" << std::endl;

                              // 将数据存入result
                              result.frame_id = (uint8_t)cat_buffer[2];
                              result.uav_index = (uint8_t)cat_buffer[3];
                              result.cmd_type = (uint8_t)cat_buffer[4];
                              // memcpy(&(result.exp_local_x), &cat_buffer[5], 8);     //exp_local_x
                              // memcpy(&(result.exp_local_y), &cat_buffer[13], 8);    //exp_local_y
                              // memcpy(&(result.exp_local_z), &cat_buffer[21], 8);    //exp_local_z
                              // memcpy(&(result.exp_local_vx), &cat_buffer[29], 8);   //exp_local_vx
                              // memcpy(&(result.exp_local_vy), &cat_buffer[37], 8);   //exp_local_vy
                              // memcpy(&(result.exp_local_vz), &cat_buffer[45], 8);   //exp_local_vz
                              // memcpy(&(result.exp_yaw), &cat_buffer[53], 4);        //exp_yaw
                              // memcpy(&(result.hour), &cat_buffer[57], 4); //hour
                              // memcpy(&(result.minutes), &cat_buffer[61], 4); //minutes
                              // memcpy(&(result.seconds), &cat_buffer[65], 4); //seconds

                              memcpy(&(result.exp_local_x), &cat_buffer[5], 8);     //exp_local_x
                              memcpy(&(result.exp_local_y), &cat_buffer[13], 8);    //exp_local_y
                              memcpy(&(result.exp_local_vx), &cat_buffer[21], 8);   //exp_local_vx
                              memcpy(&(result.exp_local_vy), &cat_buffer[29], 8);   //exp_local_vy
                              memcpy(&(result.exp_yaw), &cat_buffer[37], 8);        //exp_yaw

                              ROS_INFO_STREAM("Read: \n" << result); 
                              // memcpy(&sum_end, &cat_buffer[123], 4); //sum_end

                              // time_t t_now = time(NULL);
                              // tm* t = localtime(&t_now);
                              // int hour = t->tm_hour;
                              // int minutes = t->tm_min;
                              // int seconds = t->tm_sec;
                              // std::cout << "hour=" << hour << std::endl;
                              // std::cout << "minutes=" << minutes << std::endl;
                              // std::cout << "seconds=" << seconds << std::endl;

                              // if (result.seconds <= seconds)
                              //       time_delay.data = abs(seconds - result.seconds);
                              // else
                              //       time_delay.data = 60 - abs(seconds - result.seconds);

                              // std::cout << "time_delay=" << time_delay << std::endl;

                              read_pub.publish(result);
                              // time_delay_pub.publish(time_delay);

                              // 将剩余数据保留，
                              last_remaining_buffer.clear(); // 清空元素，buffer1.size() = 0;
                              last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + (i + LENTH_CMD_UAV), cat_buffer.end()); // 保留剩下的数据
                              break;
                              }
                        }
                        else{
                        // std::cout << "22222" << std::endl;
                        // 如果不存在完整帧，则将剩余数据保存
                        last_remaining_buffer.clear();
                        last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + i, cat_buffer.end()); // 保留剩下的数据
                        break;
                        }
                  }
            }
            // 合并两个vector，两帧拼接
            // 没找到包头，将最后两个字节数据保留，防止漏掉帧头

            if(i == data_len-2){
            // std::cout << "3333" << std::endl;
            last_remaining_buffer.clear();
            last_remaining_buffer.insert(last_remaining_buffer.end(), cat_buffer.begin() + i,  cat_buffer.end());
            }
      }

      
}

int main (int argc, char** argv) 
{ 
      //初始化节点 
      ros::init(argc, argv, "serial_port_node"); 
      //声明节点句柄 
      ros::NodeHandle nh; 
      //订阅主题，并配置回调函数
      ros::Subscriber write_sub = nh.subscribe("/outdoor/uav/state", 1, write_callback); 
      //发布主题
      ros::Publisher read_pub = nh.advertise<radio_serial::Cmd_send>("/outdoor/uav/cmd", 1);
      ros::Publisher time_delay_pub = nh.advertise<std_msgs::Int32>("/outdoor/uav/time_delay", 1); 
      try 
      { 
            //设置串口属性，并打开串口 
            ser.setPort("/dev/ttyUSB0"); 
            ser.setBaudrate(115200); 
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
      } 
      catch (serial::IOException& e) 
      { 
            ROS_ERROR_STREAM("Unable to open port "); 
            return -1; 
      } 
      //检测串口是否已经打开，并给出提示信息 
      if(ser.isOpen()) 
      { 
            ROS_INFO_STREAM("Serial Port initialized"); 
      } 
      else 
      { 
            return -1; 
      } 
      //指定循环的频率
      ros::Rate loop_rate(20);

      // while(ros::ok()) 
      // { 
      //       if(ser.available()){ 
      //             //1.读取串口信息：
      //             ROS_INFO_STREAM("Reading from serial port\n"); 
      //             //通过ROS串口对象读取串口信息
      //             // radio_serial::Cmd_send *result = (radio_serial::Cmd_send *)malloc(LENTH_CMD_UAV);
      //             radio_serial::Cmd_send result;
      //             std_msgs::Int32 time_delay;
      //             Decode_frame(ser, result, time_delay);
      //             // ROS_INFO_STREAM("Read: \n" << result);

      //             read_pub.publish(result);
      //             time_delay_pub.publish(time_delay);
      //       }
      //       //处理ROS的信息，比如订阅消息,并调用回调函数 
      //       ros::spinOnce(); 
      //       loop_rate.sleep(); 
      // }

      // 创建一个线程来运行 Decode_frame 函数
      std::thread decode_thread(Decode_frame, std::ref(ser), std::ref(read_pub), std::ref(time_delay_pub));
      decode_thread.detach(); // 将线程分离，使其在后台运行

      //处理ROS的信息，比如订阅消息,并调用回调函数 
      ros::spin(); 
}
